#include<ros/ros.h>  
#include<geometry_msgs/Twist.h>  
#include<sensor_msgs/Joy.h>  
#include<iostream>  
#include"shm.h"
#include "common_msg/Console.h"

using namespace std;  

class Teleop  
{  
public:  
    Teleop(ros::NodeHandle &nh);  
    ros::Publisher pub ; 
	Shm shm;
private:  
    /* data */  
    void callback(const sensor_msgs::Joy::ConstPtr& Joy);  
	void consoleCallback(const common_msg::Console::ConstPtr& console_msg);
    
    ros::Subscriber sub ;
	ros::Subscriber console_sub ;  
    
    double vlinear,vangular,vgain{1.0};
    int axis_ang,axis_lin; 

};  
	geometry_msgs::Twist v; 

Teleop::Teleop(ros::NodeHandle &nh)  
{     
	nh.param<int>("axis_linear",axis_lin,1);   
	nh.param<int>("axis_angular",axis_ang,0);   
	nh.param<double>("vel_linear",vlinear,0.3); 
	nh.param<double>("vel_angular",vangular,0.3);
	pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel",1); 

	sub = nh.subscribe<sensor_msgs::Joy>("/joy",1,&Teleop::callback,this); 
	console_sub = nh.subscribe("/console_msg", 2, &Teleop::consoleCallback,this); 
	int contrl_mode = 1113;
	shm.shm_init(contrl_mode,contrl_mode<<4);
	shm.shm_write("auto");
}  
float button_on_off = 1.0;

void Teleop::consoleCallback(const common_msg::Console::ConstPtr& console_msg)
{
	if(console_msg->driver_stop == 1)
	{
		ROS_INFO("shutdown gamepad");
		ros::shutdown();
	}
}
void Teleop::callback(const sensor_msgs::Joy::ConstPtr& Joy)  
{   
    if(Joy->axes[7]>0.8) 		//-1 when push, manual mode
	{
		vgain = vgain*1.2;
	}
	else if(Joy->axes[7]<-0.8)
	{
		vgain = vgain*0.83333;
	}
	float x = Joy->axes[axis_lin]*vlinear*vgain;
	float z = Joy->axes[axis_ang]*vangular*vgain;

	v.linear.x = x; 
	v.angular.z = z;  
	button_on_off = Joy->axes[2];
	static bool first_control = true;
	if(Joy->axes[2]<-0.8) 		//-1 when push, manual mode
	{
		shm.shm_write("manual");		
	}
	if(Joy->axes[5]<-0.8) 		//-1 when push, auto mode
	{
		shm.shm_write("auto");
	}
}
int main(int argc,char** argv)  
{  
	ros::init(argc, argv, "logteleop");  
	ros::NodeHandle nh("~");
	Teleop telelog(nh); 
	v.linear.x = 0.;
	v.angular.z = 0.;

	float time_use=0;
	struct timeval start;
	struct timeval end;
	gettimeofday(&start,NULL);

	while(ros::ok())
	{	
		time_use = 0.;
		gettimeofday(&start,NULL); 
		if(button_on_off < -0.8){
			telelog.pub.publish(v); 
		}
		
		gettimeofday(&end, NULL);
		time_use=(end.tv_sec-start.tv_sec)*1000000 + (end.tv_usec-start.tv_usec);
		while(time_use < 20000)						//20ms
		{
			usleep(4000);
			gettimeofday(&end, NULL);
			time_use=(end.tv_sec-start.tv_sec)*1000000 + (end.tv_usec-start.tv_usec);	
		}
		ros::spinOnce();
	}
	return 0;  
}  
